#include "ros/ros.h"
#include <geometry_msgs/Twist.h>         // 指线速度角速度的消息类型
#include <geometry_msgs/Point.h>         // 位置信息消息类型
#include <tf/transform_listener.h>       // 用于坐标转换
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>           // 用于读取里程计信息
#include <sensor_msgs/LaserScan.h>       // lidar
#include "ag_robot/PIDController.hpp"
#include "ag_robot/SimplePID.hpp"
#include <Eigen/Core>                    // 矩阵运算
#include <Eigen/QR>

// inlcude iostream and string libraries
#include <iostream>
#include <map>      // 包含了key-value键值对映射的排序分配容器
#include <math.h>
#include <string>
#include <vector>

using namespace std;
using namespace Eigen;

constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

/********************/
/* CLASS DEFINITION */
/********************/
class FollowerPIDNode
{
public:
    FollowerPIDNode(); 

    ~FollowerPIDNode(); // 析构函数
    int get_thread_numbers();   // 获取线程数

private:
    ros::NodeHandle _nh;
    ros::Subscriber _sub_odom, _sub_distance, _sub_cmd, _sub_pos, _sub_leader_vel;
    ros::Publisher _pub_twist, _pub_totalcost;
    ros::Timer _timer1;
    // 添加障碍物信息发布器
    ros::Publisher _pub_obstacle_info;

    nav_msgs::Odometry _odom, _leader_odom;
    geometry_msgs::Twist _twist_msg, _sub_twist_cmd;
    geometry_msgs::Point _sub_pos_cmd;

    // 当前跟随机器人的命名空间
    string _follower_frame; 
    string _simulation;

    // PID控制器（线速度、角速度）
    // PIDController pid_v;  
    SimplePID pid_w;  
    SimplePID pid_v;
    double _dt, _w, _speed;
    int _controller_freq, _thread_numbers;
    float _closest_distance, _closest_degree;   // 雷达测量到的最近距离和角度
    float debug_closest_range_;

    void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);               // 订阅里程计消息回调函数
    void controlLoopCB(const ros::TimerEvent&);                             // 控制回调函数
    void get_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);   // 订阅雷达消息回调函数
    void read_cmd(const geometry_msgs::Twist::ConstPtr& cmd);               // 读取用户输入的命令
    void sub_pos(const geometry_msgs::Point::ConstPtr& pos);
    void leader_odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);        // 获取领航者里程计消息

    unsigned int idx;    
}; // end of class

// 构造函数
FollowerPIDNode::FollowerPIDNode():    
    pid_v(0.8, 0.3), // kp = 0.8, kd = 0.3
    pid_w(1.2, 0.4)  // kp = 1.2, kd = 0.4
    {
    //Private parameters handler
    ros::NodeHandle pn("~");

    //Parameters for control loop
    pn.param("thread_numbers", _thread_numbers, 2);     // number of threads for this ROS node
    pn.param("controller_freq", _controller_freq, 4);   // controller_frequence
    _dt = double(1.0/_controller_freq);                 // time step duration dt in s 

    //Parameter for topics & Frame name
    pn.param<std::string>("follower_frame", _follower_frame, "tb3_1" );
    pn.param<std::string>("simulation", _simulation, "yes" );

    //Display the parameters
    cout << "\n===== Parameters =====" << endl;
    cout << "frequency: "   << _dt << endl;
    cout << "simulation: " << _simulation << endl;

    //Publishers and Subscribers
    //Tips: 
    //     1. 在当前命名空间下，订阅当前空间的内容，不需要在前面加命名空间
    //     2. 订阅内容不需要加“/” ，这是针对全局内容的

    // for combination
    if (_simulation == "yes") {
        _sub_odom = _nh.subscribe("odom", 1, &FollowerPIDNode::odomCB, this);    
    } else if (_simulation == "no") {
        _sub_odom = _nh.subscribe("fake_odom", 1, &FollowerPIDNode::odomCB, this); 
    }

    _sub_leader_vel = _nh.subscribe("/tb3_0/odom", 1, &FollowerPIDNode::leader_odomCB, this);      // 获取领航者里程计消息
    _sub_distance = _nh.subscribe("processed_scan", 1, &FollowerPIDNode::get_laser_callback, this);          // 订阅雷达消息
    if (_follower_frame == "tb3_1"){                                                                // 获取期望跟踪位置  
        // _sub_pos = _nh.subscribe("/tb3_1_pos", 1, &FollowerPIDNode::sub_pos, this);
        _sub_pos = _nh.subscribe("/tb3_1/formation_target", 1, &FollowerPIDNode::sub_pos, this);
    } else if(_follower_frame == "tb3_2") {
        // _sub_pos = _nh.subscribe("/tb3_2_pos", 1, &FollowerPIDNode::sub_pos, this);
        _sub_pos = _nh.subscribe("/tb3_2/formation_target", 1, &FollowerPIDNode::sub_pos, this);
    }
    // _sub_cmd = _nh.subscribe("/cmd_user", 1, &FollowerPIDNode::read_cmd, this);                    // 获取机器人速度命令 changed 20210114
    _sub_cmd = _nh.subscribe("/cmd_user", 1, &FollowerPIDNode::read_cmd, this);                    // 获取机器人速度命令
    _pub_twist = _nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);                                 // publish speed msg  
    // 添加障碍物信息发布器
    _pub_obstacle_info = _nh.advertise<geometry_msgs::Point>("obstacle_info", 1);

    //Timer
    _timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &FollowerPIDNode::controlLoopCB, this); // 4Hz

    idx = 0;         // 求解次数

    _w = 0.0;        // 角速度
    _speed = 0.0;    // 速度

    _closest_degree = 0;        // 初始化最近障碍物角度和距离
    _closest_distance = 3.5;
    debug_closest_range_ = 3.5;

    _twist_msg = geometry_msgs::Twist();
}

FollowerPIDNode::~FollowerPIDNode()
{
    // 析构函数  
}

// Public: return _thread_numbers
int FollowerPIDNode::get_thread_numbers()
{
    return _thread_numbers;
}

// CallBack: get positions
void FollowerPIDNode::sub_pos(const geometry_msgs::Point::ConstPtr& pos)
{
    _sub_pos_cmd = *pos;
}

// Timer: Control Loop (closed loop nonlinear MPC)
void FollowerPIDNode::controlLoopCB(const ros::TimerEvent&)
{
    // input variables
    nav_msgs::Odometry leader_odom = _leader_odom;
    nav_msgs::Odometry odom = _odom;
    geometry_msgs::Point sub_pos_cmd = _sub_pos_cmd;

    // 订阅的位置误差
    const double d_error = sub_pos_cmd.x;
    const double a_error = sub_pos_cmd.y;

    // 获取领航者速度和位置信息
    const double leader_v = leader_odom.twist.twist.linear.x; //twist: body fixed frame 
    const double leader_w = leader_odom.twist.twist.angular.z; //angular_velocity

    geometry_msgs::Twist cmd_user = _sub_twist_cmd;
    const double cmd_v = cmd_user.linear.x;       // Linear velocity
    const double cmd_w = cmd_user.angular.z;      // angular velocity

    // Update system states: X=[x, y, theta]
    // 当前机器人的位置
    const double px = odom.pose.pose.position.x; //pose: odom frame
    const double py = odom.pose.pose.position.y;
    tf::Pose pose;
    tf::poseMsgToTF(odom.pose.pose, pose);
    const double theta = tf::getYaw(pose.getRotation()); 
    const double v = odom.twist.twist.linear.x; //twist: body fixed frame 
    const double w = odom.twist.twist.angular.z; //angular_velocity

    const double cmd_obs = debug_closest_range_;
//---------------------------------------------------------------------------------------------//


    // distance and degree 距离障碍物的最近距离和角度
    const double min_distance = _closest_distance;  
    const double min_degree = _closest_degree;    

    // 使用前进方向误差 d_error 控制线速度
    double desired_v = pid_v.compute(d_error,0.1);  // _pid_v 需调整为接收 d_error

    // 替换原有的角速度 PID 计算（原代码：desired_w = pid_w.compute(0.0, error_angle);）
    // 使用垂直方向误差 a_error 控制角速度
    double desired_w = pid_w.compute(a_error,0.1);  // _pid_w 需调整为接收 a_error

    // 限制速度范围（根据机器人硬件调整）
    desired_v = std::clamp(desired_v, 0.0, 0.5);  // 最大前进速度 0.5 m/s
    desired_w = std::clamp(desired_w, -1.0, 1.0); // 最大角速度 ±1.0 rad/s

    // if (error_distance < 0.1){
    //     desired_w *= 0.2;
    // }  
    // if ((error_angle < 0.1)&&(error_angle>-0.1)){
    //     desired_w *= 0.1;
    // } 
    //  // 如果障碍物太近，减速或转向
    // if (min_distance < 0.2) {
    //     // 障碍物在左侧（角度为正）
    //     if (min_degree > 0) {
    //         desired_w = -0.01;  // 向右转
    //     } 
    //     // 障碍物在右侧（角度为负）
    //     else {
    //         desired_w = 0.01;    // 向左转
    //     }
    //     desired_v *= 0.1;      // 减速
    // }

    // MPC result (all described in car frame), output = (w)  
    // _speed = std::max(0.0,desired_v); // Linear velocity
    _speed = desired_v;
    _w = desired_w; // radian/sec, angular velocity

if ((_speed < 0.00001) && cmd_v > 0.02 && debug_closest_range_ < 0.3) {
    _speed = 0.03; 
    _w = 0.1;  // 同时添加微小转向，帮助脱离障碍物
}
//----------------------------------------------------------------------------------------------------//
    // publish general cmd_vel
    _twist_msg.linear.x  = _speed; 
    _twist_msg.angular.z = _w;
    _pub_twist.publish(_twist_msg);

    // std_msgs::Float32 mpc_total_cost;
    // mpc_total_cost.data = static_cast<float>(_mpc._mpc_totalcost);
    // _pub_totalcost.publish(mpc_total_cost);

    idx++;
}

// CallBack: Update odometry
void FollowerPIDNode::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    _odom = *odomMsg;
}

// 获取领航者odom
void FollowerPIDNode::leader_odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    _leader_odom = *odomMsg;
}

// 读取用户输入的命令 
void FollowerPIDNode::read_cmd(const geometry_msgs::Twist::ConstPtr& cmd)
{
    _sub_twist_cmd = *cmd;
}

// 获取雷达最近距离和角度
void FollowerPIDNode::get_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    // 获取距离数组
    std::vector<float> ranges=msg->ranges;
    float angle_min1 = msg -> angle_min;
    float angle_max1 = msg -> angle_max;

    // 获取距离数组的长度
    int len_ = ranges.size();

    // 距离数组标准化
    for(int i=0 ; i < len_ ; i++)
    {
        if(isinf(ranges[i]))
        {
            ranges[i] = 3.5;
        }
        else if(isnan(ranges[i]))
        {
            ranges[i] = 0.0;
        }
    }

    // 求最短距离
    auto closest_range = std::min_element(ranges.begin(),ranges.end());
    float closest_range_ = *(closest_range);

    // 求最短距离所在位置(角度rad)
    auto closest_range_seq = closest_range - ranges.begin() ;
    float closest_degree_rad = angle_min1 + closest_range_seq*(angle_max1-angle_min1)/len_  ;

    // debug
    debug_closest_range_ = closest_range_;

    // changed
    // if((1.05 < closest_degree_rad)&&(closest_degree_rad < 5.23))
    // if((0.785 < closest_degree_rad)&&(closest_degree_rad < 5.495))
    if((1.05 < closest_degree_rad)&&(closest_degree_rad < 5.23))
    {
        closest_range_ = 3.5;
    }
    float closest_degree = rad2deg(closest_degree_rad);

    _closest_distance = closest_range_;
    _closest_degree = closest_degree_rad;

    // 发布障碍物信息
    geometry_msgs::Point obstacle_msg;
    obstacle_msg.x = _closest_distance;    // 距离
    obstacle_msg.y = _closest_degree;       // 角度（弧度）
    // obstacle_msg.z = rad2deg(_closest_degree); // 角度（度，可选）
    
    _pub_obstacle_info.publish(obstacle_msg);
    
    // 调试信息（可选）
    ROS_INFO("最近障碍物: 距离=%.2fm, 角度=%.2frad(%.1f°)", 
             _closest_distance, _closest_degree, rad2deg(_closest_degree));
}

/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "FollowerPIDNode");

    FollowerPIDNode FollowerPID_node;

    ros::AsyncSpinner spinner(FollowerPID_node.get_thread_numbers()); // Use multi threads
    spinner.start();
    ros::waitForShutdown();
    return 0;
}
